// ---------------------------------------------------------------------------

#pragma hdrstop

#include "Motor.h"
#include "math.h"
#include "GeneralParameter.h"
// ---------------------------------------------------------------------------
#pragma package(smart_init)
// ---------------------------------------------------------------------------

/** **************************************************************************
 ** Motor[index]->ID
 ** index of Motor may be different to ID of Motor. Therefore,
 ** the value of table_p and the send data should be transfer
 ** to actual ID.
 *************************************************************************** */
// ---------------------------------------------------------------------------

Motor::Motor(TWinControl *Sender, int id) {
	this->ID = id;
	this->RPM = 0;
	this->Direction = DIR_UP;
	this->PartDriven = TURNTABLE_DRIVEN;
	this->SetPolarity(this->Direction, this->PartDriven);
	this->BatteryGroupNo = UPPER_BODY;

	this->JointNo = -1;
	this->AngleToNextJoint = 0;

	this->Position_Max = 1024;
	this->Position_Min = 1;

	this->Position = ceil((float)(this->Position_Max -this->Position_Min) / 2);
	this->Speed = ceil((float)(this->Speed_Max -this->Speed_Min) / 2);

	this->Component = new TMotorComponent(Sender, id, this->Position_Min,
		this->Position_Max);

//	this->Type = EX_106P;
//	this->SetPositionLimit();
	this->SetType(EX_106P);

	// The OutputData must be initialized or assigned value, before
	// being send to control board by Zigbee.
	// Otherwise, the control board will received that the position
	// value is set to 0. It will cause a very dangerous situation.
	this->OutputData[POSITION_LOW] = this->Position & 0x00ff;
	this->OutputData[POSITION_LOW] = (this->Position & 0xff00) >> 8;
	this->OutputData[TORQUE_ENABLE] = true;
	this->OutputData[POSITION_CHANGE] = false;
	this->OutputData[TORQUE_CHANGE] = false;

	this->InuputPosition = this->Position;

	this->RotatePolarity = POSITIVE;
	this->AngleBias = 0;
}
// ---------------------------------------------------------------------------

Motor::~Motor() {
	delete this->Component;
}
// ---------------------------------------------------------------------------

void Motor::SetComponent(int id, ComponentDirStatus Dir, int Top, int Left) {
	int Min = this->Position_Min;
	int Max = this->Position_Max;
	this->Component->SetProperties(id, Dir, Top, Left, Min, Max);
}
// ---------------------------------------------------------------------------

void Motor::SetDirection(DirectionStatus dir) {
	this->Direction = dir;
	this->SetPolarity(this->Direction, this->PartDriven);
}
// ---------------------------------------------------------------------------

void Motor::SetPartDriven(PartDrivenStatus partdriven) {
	this->PartDriven = partdriven;
	this->SetPolarity(this->Direction, this->PartDriven);
}
// ---------------------------------------------------------------------------

DirectionStatus Motor::GetDirection() {
	return this->Direction;
}
// ---------------------------------------------------------------------------

PartDrivenStatus Motor::GetPartDriven() {
	return this->PartDriven;
}
// ---------------------------------------------------------------------------

PolarityStatus Motor::LookupPolarity(DirectionStatus dir,
	PartDrivenStatus partdriven) {
	PolarityStatus polarity;

	if (partdriven == TURNTABLE_DRIVEN) {
		if (dir == DIR_UP) {
			polarity = POSITIVE;
		}
		else if (dir == DIR_DOWN) {
			polarity = NEGATIVE;
		}
		else if (dir == DIR_RIGHT) {
			polarity = NEGATIVE;
		}
		else if (dir == DIR_LEFT) {
			polarity = POSITIVE;
		}
		else if (dir == DIR_FORWARD) {
			polarity = POSITIVE;
		}
		else // if(dir == DIR_BACKWARD)
		{
			polarity = NEGATIVE;
		}
	}
	else // if(partdriven == SHELL_DRIVEN)
	{
		if (dir == DIR_UP) {
			polarity = NEGATIVE;
		}
		else if (dir == DIR_DOWN) {
			polarity = POSITIVE;
		}
		else if (dir == DIR_RIGHT) {
			polarity = POSITIVE;
		}
		else if (dir == DIR_LEFT) {
			polarity = NEGATIVE;
		}
		else if (dir == DIR_FORWARD) {
			polarity = NEGATIVE;
		}
		else // if(dir == DIR_BACKWARD)
		{
			polarity = POSITIVE;
		}
	}
	return polarity;
}
// ---------------------------------------------------------------------------

void Motor::SetPolarity(DirectionStatus dir, PartDrivenStatus partdriven) {
	this->MoveUpPolarity = this->LookupPolarity(dir, partdriven);
}
// ---------------------------------------------------------------------------

PolarityStatus Motor::GetPolarity() {
	return this->MoveUpPolarity;
}
// ---------------------------------------------------------------------------

void Motor::SetOutputPosition(int position) {
	this->Position = this->bound_position(this->Position);
	this->OutputData[POSITION_LOW] = this->Position & 0x00ff;
	this->OutputData[POSITION_HIGH] = (this->Position & 0xff00) >> 8;
	this->OutputData[POSITION_CHANGE] = true;
}
// ---------------------------------------------------------------------------

void Motor::SetOutputData(OutPutDataDef index, unsigned char value) {
	if ((index >= 0) && (index < OUTPUT_DATA_LENGHT)) {
		this->OutputData[index] = value;
	}
}
// ---------------------------------------------------------------------------

unsigned char Motor::GetOutputData(OutPutDataDef index) {
	if ((index >= 0) && (index < OUTPUT_DATA_LENGHT)) {
		return this->OutputData[index];
	}
	else {
		return 0;
	}
}
// ---------------------------------------------------------------------------

int Motor::bound_position(int value) {
	if (value > this->Position_Max)
		return this->Position_Max;
	else if (value < this->Position_Min)
		return this->Position_Min;
	else
		return value;
}
// ---------------------------------------------------------------------------

int Motor::GetPosition() {
	return this->Position;
}
// ---------------------------------------------------------------------------

void Motor::ComponentBar_Change(TObject *Sender) {
	// Becasue the position of MBar is already bounded when MBar constructed
	// , there is no need to call bound_position()

	// Set the value of position according to MoveUpPolarity.
	if (this->MoveUpPolarity == POSITIVE) {
		this->Position = this->Component->MBar->Position;
	}
	else // if(this->MoveUpPolarity == NEGATIVE)
	{
		this->Position = this->Position_Max -this->Component->MBar->Position;
	}

	this->Component->MEdit->Text = IntToStr(this->Position);

	// If the torque of motor is enable, renew the output position.
	if (!this->Component->MCheck->Checked) {
		this->SetOutputPosition(this->Position);
	}

	// If the position is changed and is different to the original
	// value, the color of MEdit is changed as well.
	if (this->Position == this->InuputPosition) {
		this->Component->MEdit->Color = clWindow;
	}
	else {
		this->Component->MEdit->Color = (TColor)0x0099F0F2;
	}
}

// ---------------------------------------------------------------------------

void Motor::ComponentButton_Click(TObject *Sender) {
	int multiple = 1;
	if (Sender == this->Component->MButtonP) {
		multiple = this->GetPolarity() * EditMultiple;
	}
	else // if(Sender == this->Component->MButtonN)
	{
		multiple = -1 * this->GetPolarity() * EditMultiple;
	}

	this->Position = StrToInt(this->Component->MEdit->Text) + multiple;
	this->Position = this->bound_position(this->Position);
	this->Component->MEdit->Text = IntToStr(this->Position);

	if (this->MoveUpPolarity == POSITIVE) {
		this->Component->MBar->Position = this->Position;
	}
	else // if(this->MoveUpPolarity == NEGATIVE)
	{
		this->Component->MBar->Position = this->Position_Max -this->Position;
	}

	// If position is changed, the remaining task will be completed
	// by the function ComponentBar_Change()
}
// ---------------------------------------------------------------------------

void Motor::ComponentEdit_KeyPress(TObject *Sender, wchar_t &Key) {
	if (Key == 13) {
		this->Position = StrToInt(this->Component->MEdit->Text);
		this->Position = this->bound_position(this->Position);
		this->Component->MEdit->Text = IntToStr(this->Position);

		if (this->MoveUpPolarity == POSITIVE) {
			this->Component->MBar->Position = this->Position;
		}
		else // if(this->MoveUpPolarity == NEGATIVE)
		{
			this->Component->MBar->Position = this->Position_Max -this->Position;
		}

		// If position is changed, the remaining task will be
		// completed by the function ComponentBar_Change()
	}
}
// ---------------------------------------------------------------------------

void Motor::ComponentCheck_Click(TObject *Sender) {

	if (this->Component->MCheck->Checked) {
		this->Component->MEdit->Enabled = false;
		this->SetOutputData(TORQUE_ENABLE, false);
	}
	else {
		this->Component->MEdit->Enabled = true;
		this->SetOutputData(TORQUE_ENABLE, true);
	}
	this->SetOutputData(TORQUE_CHANGE, true);
}

// ---------------------------------------------------------------------------
void Motor::SetPositionLimit() {
	int temp_position = this->Position;
	if ((this->Type == EX_106P) || (this->Type == MX_106)) {
		this->Position_Min = 1;
		this->Position_Max = 4096;
	}
	else {
		this->Position_Min = 1;
		this->Position_Max = 1024;
	}
	this->Component->SetPositionLimit(this->Position_Min, this->Position_Max);
	this->Position = this->bound_position(temp_position);
	this->SetOutputPosition(this->Position);
}

// ---------------------------------------------------------------------------
void Motor::SetType(MotorType mtype) {
	this->Type = mtype;
	this->SetPositionLimit();

	switch(mtype)
	{
		case RX_64:
			this->Resolution = Rx64_Resolution;
			this->RunningDegree = Rx64_RunningDegree;
			this->SpeedUnit = Rx64_SpeedUnit;
			this->MaxSpeed = Rx64_MaxSpeed;
		break;

		case EX_106P:
			this->Resolution = Ex106_Resolution;
			this->RunningDegree = Ex106_RunningDegree;
			this->SpeedUnit = Ex106_SpeedUnit;
			this->MaxSpeed = Ex106_MaxSpeed;
		break;

		case MX_106:
			this->Resolution = Mx106_Resolution;
			this->RunningDegree = Mx106_RunningDegree;
			this->SpeedUnit = Mx106_SpeedUnit;
			this->MaxSpeed = Mx106_MaxSpeed;
		break;

		default: // RX_28
			this->Resolution = Rx28_Resolution;
			this->RunningDegree = Rx28_RunningDegree;
			this->SpeedUnit = Rx28_SpeedUnit;
			this->MaxSpeed = Rx28_MaxSpeed;
		break;
  }
}

// ---------------------------------------------------------------------------
MotorType Motor::GetType()
{
	return this->Type;
}

// ---------------------------------------------------------------------------
int Motor::GetPositionMin() {
	return this->Position_Min;
}

// ---------------------------------------------------------------------------
int Motor::GetPositionMax() {
	return this->Position_Max;
}

// ---------------------------------------------------------------------------
float Motor::GetResolution()
{
	return this->Resolution;
}

// ---------------------------------------------------------------------------
float Motor::GetRunningDegree()
{
	return this->RunningDegree;
}

// ---------------------------------------------------------------------------
float Motor::GetSpeedUnit()
{
	return this->SpeedUnit;
}

// ---------------------------------------------------------------------------
float Motor::GetMaxSpeed()
{
	return this->MaxSpeed;
}

// ---------------------------------------------------------------------------
